/*  NVS: non volatile storage in flash
 *
 * Copyright (c) 2018 Laczen
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-03     Liuis        Ported from zephyr RTOS. 
 */

#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include "nvs.h"
#include "nvs_priv.h"

#define     MIN(a,b)            (((a)>(b))?(b):(a))

static const uint8_t crc8_ccitt_small_table[16] =
{
    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15,
    0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d
};

static uint8_t crc8_ccitt(uint8_t val, const void *buf, size_t cnt)
{
    size_t i;
    const uint8_t *p = buf;

    for (i = 0; i < cnt; i++)
    {
        val ^= p[i];
        val = (val << 4) ^ crc8_ccitt_small_table[val >> 4];
        val = (val << 4) ^ crc8_ccitt_small_table[val >> 4];
    }
    return val;
}

static int nvs_prev_ate(struct nvs_fs *fs, uint32_t *addr, struct nvs_ate *ate);
static int nvs_ate_valid(struct nvs_fs *fs, const struct nvs_ate *entry);

#if CONFIG_NVS_LOOKUP_CACHE

/* find specified id in lookup cache. if found, put ate address to @ate_addr.
 * return 1 if id was found, otherwise 0.
 */
static int nvs_lookup_cache_find(struct nvs_fs *fs, uint16_t id, uint32_t *ate_addr)
{
    if (id == NVS_ID_UNUSED || id == NVS_ID_DELETED)
    {
        return 0;
    }

    struct lookup_cache *cache_entry = fs->lookup_cache;
    uint32_t i;
    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if ((cache_entry->id == id))
        {
            *ate_addr = cache_entry->addr;
            return 1;
        }
        if (cache_entry->id == NVS_ID_UNUSED)
        {
            return 0;
        }
        cache_entry++;
    }

    return 0;
}

static int nvs_lookup_cache_is_full(struct nvs_fs *fs)
{
    uint32_t i;
    struct lookup_cache *cache_entry = fs->lookup_cache;

    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if (cache_entry->id == NVS_ID_UNUSED || cache_entry->id == NVS_ID_DELETED)
        {
            return 0;
        }
        cache_entry++;
    }

    return 1;
}

static void nvs_lookup_cache_add(struct nvs_fs *fs, uint16_t id, uint32_t ate_addr)
{
    if (id == NVS_ID_UNUSED || id == NVS_ID_DELETED)
    {
        return;
    }

    struct lookup_cache *cache_entry = fs->lookup_cache;
    uint32_t i;
    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if (cache_entry->id == NVS_ID_UNUSED || cache_entry->id == NVS_ID_DELETED)
        {
            cache_entry->id = id;
            cache_entry->addr = ate_addr;
            break;
        }
        cache_entry++;
    }
}

static void nvs_lookup_cache_wrt(struct nvs_fs *fs, uint16_t id, uint32_t ate_addr)
{
    if (id == NVS_ID_UNUSED || id == NVS_ID_DELETED)
    {
        return;
    }

    struct lookup_cache *cache_entry = fs->lookup_cache;
    uint32_t i;
    uint16_t pos = 0xFFFF;
    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if (cache_entry->id == NVS_ID_UNUSED || cache_entry->id == id)
        {
            cache_entry->id = id;
            cache_entry->addr = ate_addr;
            break;
        }
        else if (cache_entry->id == NVS_ID_DELETED && pos == 0xFFFF)
        {
            pos = i;
        }
        cache_entry++;
    }
    if (i == CONFIG_NVS_LOOKUP_CACHE_SIZE && pos != 0xFFFF)
    {
        fs->lookup_cache[i].id = id;
        fs->lookup_cache[i].addr = ate_addr;
    }
}

static void nvs_lookup_cache_invalidate(struct nvs_fs *fs, uint32_t sector)
{
    struct lookup_cache *cache_entry = fs->lookup_cache;
    uint32_t i;

    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if ((cache_entry->addr >> ADDR_SECT_SHIFT) == sector)
        {
            cache_entry->id = NVS_ID_DELETED;
            cache_entry->addr = NVS_LOOKUP_CACHE_NO_ADDR;
            break;
        }
    }
}

static int nvs_lookup_cache_rebuild(struct nvs_fs *fs)
{
    int rc;
    uint32_t addr, temp_addr, ate_addr, n;
    struct lookup_cache *cache_entry;
    struct nvs_ate ate;

    memset(fs->lookup_cache, 0xff, sizeof(fs->lookup_cache));
    addr = fs->ate_wra;
    cache_entry = fs->lookup_cache;
    n = 0;

    while (true)
    {
        /* Make a copy of 'addr' as it will be advanced by nvs_pref_ate() */
        ate_addr = addr;
        rc = nvs_prev_ate(fs, &addr, &ate);

        if (rc)
        {
            return rc;
        }

        /* ate is valid and ate id is not in lookup cache */
        if (ate.id != NVS_ID_UNUSED && nvs_ate_valid(fs, &ate) &&
            !nvs_lookup_cache_find(fs, ate.id, &temp_addr))
        {
            nvs_lookup_cache_wrt(fs, ate.id, ate_addr);
            n++;
        }

        if (addr == fs->ate_wra)
        {
            break;
        }
        if (n == CONFIG_NVS_LOOKUP_CACHE_SIZE)
        {
            break;
        }
    }

    return 0;
}

#endif /* CONFIG_NVS_LOOKUP_CACHE */

/* basic routines */
/* nvs_al_size returns size aligned to fs->write_unit */
static inline size_t nvs_al_size(struct nvs_fs *fs, size_t len)
{
    uint8_t write_unit = fs->flash_params->write_unit;

    if (write_unit <= 1U)
    {
        return len;
    }
    return (len + (write_unit - 1U)) & ~(write_unit - 1U);
}
/* end basic routines */

/* flash routines */
/* basic aligned flash write to nvs address */
static int nvs_flash_al_wrt(struct nvs_fs *fs, uint32_t addr, const void *data,
                            size_t len)
{
    const uint8_t *data8 = (const uint8_t *)data;
    int rc = 0;
    uint32_t offset;
    size_t blen;
    uint8_t buf[NVS_BUFFER_SIZE];

    if (!len)
    {
        /* Nothing to write, avoid changing the flash protection */
        return 0;
    }

    offset = fs->offset;
    offset += fs->sector_size * (addr >> ADDR_SECT_SHIFT);
    offset += addr & ADDR_OFFS_MASK;

    blen = len & ~(fs->flash_params->write_unit - 1U);
    if (blen > 0)
    {
        rc = fs->flash_params->write(offset, data8, blen);
        if (rc)
        {
            /* flash write error */
            goto end;
        }
        len -= blen;
        offset += blen;
        data8 += blen;
    }
    if (len)
    {
        memcpy(buf, data8, len);
        (void)memset(buf + len, fs->flash_params->erase_value,
                    fs->flash_params->write_unit - len);

        rc = fs->flash_params->write(offset, buf, fs->flash_params->write_unit);
    }

end:
    return rc;
}

/* basic flash read from nvs address */
static int nvs_flash_rd(struct nvs_fs *fs, uint32_t addr, void *data,
                        size_t len)
{
    int rc;
    uint32_t offset;

    offset = fs->offset;
    offset += fs->sector_size * (addr >> ADDR_SECT_SHIFT);
    offset += addr & ADDR_OFFS_MASK;

    rc = fs->flash_params->read(offset, data, len);
    return rc;
}

/* allocation entry write */
static int nvs_flash_ate_wrt(struct nvs_fs *fs, const struct nvs_ate *entry)
{
    int rc;

    rc = nvs_flash_al_wrt(fs, fs->ate_wra, entry, sizeof(struct nvs_ate));

#if CONFIG_NVS_LOOKUP_CACHE
    /* 0xFFFF is a special-purpose identifier. Exclude it from the cache */
    if (entry->id != 0xFFFF)
    {
        nvs_lookup_cache_wrt(fs, entry->id, fs->ate_wra);
    }
#endif

    fs->ate_wra -= nvs_al_size(fs, sizeof(struct nvs_ate));

    return rc;
}

/* data write */
static int nvs_flash_data_wrt(struct nvs_fs *fs, const void *data, size_t len)
{
    int rc;

    rc = nvs_flash_al_wrt(fs, fs->data_wra, data, len);
    fs->data_wra += nvs_al_size(fs, len);

    return rc;
}

/* flash ate read */
static int nvs_flash_ate_rd(struct nvs_fs *fs, uint32_t addr,
                            struct nvs_ate *entry)
{
    return nvs_flash_rd(fs, addr, entry, sizeof(struct nvs_ate));
}

/* end of basic flash routines */

/* advanced flash routines */

/* nvs_flash_block_cmp compares the data in flash at addr to data
 * in blocks of size NVS_BUFFER_SIZE aligned to fs->write_unit
 * returns 0 if equal, 1 if not equal, errcode if error
 */
static int nvs_flash_block_cmp(struct nvs_fs *fs, uint32_t addr, const void *data,
                                size_t len)
{
    const uint8_t *data8 = (const uint8_t *)data;
    int rc;
    size_t bytes_to_cmp, rd_size;
    uint8_t buf[NVS_BUFFER_SIZE];

    rd_size = NVS_BUFFER_SIZE & ~(fs->flash_params->write_unit - 1U);

    while (len)
    {
        bytes_to_cmp = MIN(rd_size, len);
        rc = nvs_flash_rd(fs, addr, buf, bytes_to_cmp);
        if (rc)
        {
            return rc;
        }
        rc = memcmp(data8, buf, bytes_to_cmp);
        if (rc)
        {
            return 1;
        }
        len -= bytes_to_cmp;
        addr += bytes_to_cmp;
        data8 += bytes_to_cmp;
    }
    return 0;
}

/* nvs_flash_cmp_const compares the data in flash at addr to a constant
 * value. returns 0 if all data in flash is equal to value, 1 if not equal,
 * errcode if error
 */
static int nvs_flash_cmp_const(struct nvs_fs *fs, uint32_t addr, uint8_t value,
                                size_t len)
{
    int rc;
    size_t bytes_to_cmp, rd_size;
    uint8_t cmp[NVS_BUFFER_SIZE];

    rd_size = NVS_BUFFER_SIZE & ~(fs->flash_params->write_unit - 1U);

    (void)memset(cmp, value, rd_size);
    while (len)
    {
        bytes_to_cmp = MIN(rd_size, len);
        rc = nvs_flash_block_cmp(fs, addr, cmp, bytes_to_cmp);
        if (rc)
        {
            return rc;
        }
        len -= bytes_to_cmp;
        addr += bytes_to_cmp;
    }
    return 0;
}

/* flash block move: move a block at addr to the current data write location
 * and updates the data write location.
 */
static int nvs_flash_block_move(struct nvs_fs *fs, uint32_t addr, size_t len)
{
    int rc;
    size_t bytes_to_copy, rd_size;
    uint8_t buf[NVS_BUFFER_SIZE];

    rd_size = NVS_BUFFER_SIZE & ~(fs->flash_params->write_unit - 1U);

    while (len)
    {
        bytes_to_copy = MIN(rd_size, len);
        rc = nvs_flash_rd(fs, addr, buf, bytes_to_copy);
        if (rc)
        {
            return rc;
        }
        rc = nvs_flash_data_wrt(fs, buf, bytes_to_copy);
        if (rc)
        {
            return rc;
        }
        len -= bytes_to_copy;
        addr += bytes_to_copy;
    }
    return 0;
}

/* erase a sector and verify erase was OK.
 * return 0 if OK, errorcode on error.
 */
static int nvs_flash_erase_sector(struct nvs_fs *fs, uint32_t addr)
{
    int rc;
    uint32_t offset;

    addr &= ADDR_SECT_MASK;

    offset = fs->offset;
    offset += fs->sector_size * (addr >> ADDR_SECT_SHIFT);

    NVS_LOG_DBG("Erasing flash at %08x, len %d.\n", offset, fs->sector_size);

#if CONFIG_NVS_LOOKUP_CACHE
    nvs_lookup_cache_invalidate(fs, addr >> ADDR_SECT_SHIFT);
#endif
    rc = fs->flash_params->erase(offset, fs->sector_size);

    if (rc)
    {
        return rc;
    }

    if (nvs_flash_cmp_const(fs, addr, fs->flash_params->erase_value,
                            fs->sector_size))
    {
        rc = NVS_ERR_ERASE;
    }

    return rc;
}

/* crc update on allocation entry */
static void nvs_ate_crc8_update(struct nvs_ate *entry)
{
    uint8_t crc8;

    crc8 = crc8_ccitt(0xff, entry, offsetof(struct nvs_ate, crc8));
    entry->crc8 = crc8;
}

/* crc check on allocation entry
 * returns 0 if OK, 1 on crc fail
 */
static int nvs_ate_crc8_check(const struct nvs_ate *entry)
{
    uint8_t crc8;

    crc8 = crc8_ccitt(0xff, entry, offsetof(struct nvs_ate, crc8));
    if (crc8 == entry->crc8)
    {
        return 0;
    }
    return 1;
}

/* nvs_ate_cmp_const compares an ATE to a constant value. returns 0 if
 * the whole ATE is equal to value, 1 if not equal.
 */
static int nvs_ate_cmp_const(const struct nvs_ate *entry, uint8_t value)
{
    const uint8_t *data8 = (const uint8_t *)entry;
    int i;

    for (i = 0; i < sizeof(struct nvs_ate); i++)
    {
        if (data8[i] != value)
        {
            return 1;
        }
    }

    return 0;
}

/* nvs_ate_valid validates an ate:
 *     return 1 if crc8 and offset valid,
 *            0 otherwise
 */
static int nvs_ate_valid(struct nvs_fs *fs, const struct nvs_ate *entry)
{
    size_t ate_size;

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    if ((nvs_ate_crc8_check(entry)) ||
        (entry->offset >= (fs->sector_size - ate_size)))
    {
        return 0;
    }

    return 1;
}

/* nvs_close_ate_valid validates an sector close ate: a valid sector close ate:
 * - valid ate
 * - len = 0 and id = 0xFFFF
 * - offset points to location at ate multiple from sector size
 * return 1 if valid, 0 otherwise
 */
static int nvs_close_ate_valid(struct nvs_fs *fs, const struct nvs_ate *entry)
{
    size_t ate_size;

    if ((!nvs_ate_valid(fs, entry)) || (entry->len != 0U) ||
        (entry->id != 0xFFFF))
    {
        return 0;
    }

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));
    if ((fs->sector_size - entry->offset) % ate_size)
    {
        return 0;
    }

    return 1;
}

/* store an entry in flash */
static int nvs_flash_wrt_entry(struct nvs_fs *fs, uint16_t id, const void *data,
                                size_t len)
{
    int rc;
    struct nvs_ate entry;

    entry.id = id;
    entry.offset = (uint16_t)(fs->data_wra & ADDR_OFFS_MASK);
    entry.len = (uint16_t)len;
    entry.part = 0xff;

    nvs_ate_crc8_update(&entry);

    rc = nvs_flash_data_wrt(fs, data, len);
    if (rc)
    {
        return rc;
    }
    rc = nvs_flash_ate_wrt(fs, &entry);
    if (rc)
    {
        return rc;
    }

    return 0;
}
/* end of flash routines */

/* If the closing ate is invalid, its offset cannot be trusted and
 * the last valid ate of the sector should instead try to be recovered by going
 * through all ate's.
 *
 * addr should point to the faulty closing ate and will be updated to the last
 * valid ate. If no valid ate is found it will be left untouched.
 */
static int nvs_recover_last_ate(struct nvs_fs *fs, uint32_t *addr)
{
    uint32_t data_end_addr, ate_end_addr;
    struct nvs_ate end_ate;
    size_t ate_size;
    int rc;

    NVS_LOG_DBG("Recovering last ate from sector %d.\n",
            (*addr >> ADDR_SECT_SHIFT));

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    *addr -= ate_size;
    ate_end_addr = *addr;
    data_end_addr = *addr & ADDR_SECT_MASK;
    while (ate_end_addr > data_end_addr)
    {
        rc = nvs_flash_ate_rd(fs, ate_end_addr, &end_ate);
        if (rc)
        {
            return rc;
        }
        if (nvs_ate_valid(fs, &end_ate))
        {
            /* found a valid ate, update data_end_addr and *addr */
            data_end_addr &= ADDR_SECT_MASK;
            data_end_addr += end_ate.offset + end_ate.len;
            *addr = ate_end_addr;
        }
        ate_end_addr -= ate_size;
    }

    return 0;
}

/* walking through allocation entry list, from newest to oldest entries
 * read ate from addr, modify addr to the previous ate
 */
static int nvs_prev_ate(struct nvs_fs *fs, uint32_t *addr, struct nvs_ate *ate)
{
    int rc;
    struct nvs_ate close_ate;
    size_t ate_size;

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    rc = nvs_flash_ate_rd(fs, *addr, ate);
    if (rc)
    {
        return rc;
    }

    *addr += ate_size;
    /* not the last ate in sector, return */
    if (((*addr) & ADDR_OFFS_MASK) != (fs->sector_size - ate_size))
    {
        return 0;
    }

    /* last ate in sector, do jump to previous sector */
    if (((*addr) >> ADDR_SECT_SHIFT) == 0U)
    {
        *addr += ((fs->sector_count - 1) << ADDR_SECT_SHIFT);
    }
    else
    {
        *addr -= (1 << ADDR_SECT_SHIFT);
    }

    rc = nvs_flash_ate_rd(fs, *addr, &close_ate);
    if (rc)
    {
        return rc;
    }

    rc = nvs_ate_cmp_const(&close_ate, fs->flash_params->erase_value);
    /* at the end of filesystem */
    if (!rc)
    {
        *addr = fs->ate_wra;
        return 0;
    }

    /* Update the address if the close ate is valid.
     */
    if (nvs_close_ate_valid(fs, &close_ate))
    {
        (*addr) &= ADDR_SECT_MASK;
        (*addr) += close_ate.offset;
        return 0;
    }

    /* The close_ate was invalid, `lets find out the last valid ate
     * and point the address to this found ate.
     *
     * remark: if there was absolutely no valid data in the sector *addr
     * is kept at sector_end - 2*ate_size, the next read will contain
     * invalid data and continue with a sector jump
     */
    return nvs_recover_last_ate(fs, addr);
}

static void nvs_sector_advance(struct nvs_fs *fs, uint32_t *addr)
{
    *addr += (1 << ADDR_SECT_SHIFT);
    if ((*addr >> ADDR_SECT_SHIFT) == fs->sector_count)
    {
        *addr -= (fs->sector_count << ADDR_SECT_SHIFT);
    }
}

/* allocation entry close (this closes the current sector) by writing offset
 * of last ate to the sector end.
 */
static int nvs_sector_close(struct nvs_fs *fs)
{
    int rc;
    struct nvs_ate close_ate;
    size_t ate_size;

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    close_ate.id = 0xFFFF;
    close_ate.len = 0U;
    close_ate.offset = (uint16_t)((fs->ate_wra + ate_size) & ADDR_OFFS_MASK);
    close_ate.part = 0xff;

    fs->ate_wra &= ADDR_SECT_MASK;
    fs->ate_wra += (fs->sector_size - ate_size);

    nvs_ate_crc8_update(&close_ate);

    rc = nvs_flash_ate_wrt(fs, &close_ate);

    nvs_sector_advance(fs, &fs->ate_wra);

    fs->data_wra = fs->ate_wra & ADDR_SECT_MASK;

    return 0;
}

static int nvs_add_gc_done_ate(struct nvs_fs *fs)
{
    struct nvs_ate gc_done_ate;

    NVS_LOG_DBG("Adding gc done ate at %08x.\n", fs->ate_wra);
    gc_done_ate.id = 0xffff;
    gc_done_ate.len = 0U;
    gc_done_ate.part = 0xff;
    gc_done_ate.offset = (uint16_t)(fs->data_wra & ADDR_OFFS_MASK);
    nvs_ate_crc8_update(&gc_done_ate);

    return nvs_flash_ate_wrt(fs, &gc_done_ate);
}

/* garbage collection: the address ate_wra has been updated to the new sector
 * that has just been started. The data to gc is in the sector after this new
 * sector.
 */
static int nvs_gc(struct nvs_fs *fs)
{
    int rc;
    struct nvs_ate close_ate, gc_ate, wlk_ate;
    uint32_t sec_addr, gc_addr, gc_prev_addr, wlk_addr, wlk_prev_addr,
        data_addr, stop_addr;
    size_t ate_size;

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    sec_addr = (fs->ate_wra & ADDR_SECT_MASK);
    nvs_sector_advance(fs, &sec_addr);
    gc_addr = sec_addr + fs->sector_size - ate_size;

    /* if the sector is not closed don't do gc */
    rc = nvs_flash_ate_rd(fs, gc_addr, &close_ate);
    if (rc < 0)
    {
        /* flash error */
        return rc;
    }

    rc = nvs_ate_cmp_const(&close_ate, fs->flash_params->erase_value);
    if (!rc)
    {
        goto gc_done;
    }

    stop_addr = gc_addr - ate_size;

    if (nvs_close_ate_valid(fs, &close_ate))
    {
        gc_addr &= ADDR_SECT_MASK;
        gc_addr += close_ate.offset;
    }
    else
    {
        rc = nvs_recover_last_ate(fs, &gc_addr);
        if (rc)
        {
            return rc;
        }
    }

    do
    {
        gc_prev_addr = gc_addr;
        rc = nvs_prev_ate(fs, &gc_addr, &gc_ate);
        if (rc)
        {
            return rc;
        }

        if (!nvs_ate_valid(fs, &gc_ate))
        {
            continue;
        }

#if CONFIG_NVS_LOOKUP_CACHE
        if (!nvs_lookup_cache_find(fs, gc_ate.id, &wlk_addr))
        {
            wlk_addr = fs->ate_wra;
        }
#else
        wlk_addr = fs->ate_wra;
#endif
        do
        {
            wlk_prev_addr = wlk_addr;
            rc = nvs_prev_ate(fs, &wlk_addr, &wlk_ate);
            if (rc)
            {
                return rc;
            }
            /* if ate with same id is reached we might need to copy.
             * only consider valid wlk_ate's. Something wrong might
             * have been written that has the same ate but is
             * invalid, don't consider these as a match.
             */
            if ((wlk_ate.id == gc_ate.id) &&
                (nvs_ate_valid(fs, &wlk_ate)))
            {
                break;
            }
        } while (wlk_addr != fs->ate_wra);

        /* if walk has reached the same address as gc_addr copy is
         * needed unless it is a deleted item.
         */
        if ((wlk_prev_addr == gc_prev_addr) && gc_ate.len)
        {
            /* copy needed */

            data_addr = (gc_prev_addr & ADDR_SECT_MASK);
            data_addr += gc_ate.offset;

            gc_ate.offset = (uint16_t)(fs->data_wra & ADDR_OFFS_MASK);
            nvs_ate_crc8_update(&gc_ate);

            NVS_LOG_DBG("Moving data. id=0x%04x, len=%d. From 0x%08x to 0x%08x.\n", 
                    gc_ate.id, gc_ate.len, data_addr, fs->data_wra);
            rc = nvs_flash_block_move(fs, data_addr, gc_ate.len);
            if (rc)
            {
                return rc;
            }

            rc = nvs_flash_ate_wrt(fs, &gc_ate);
            if (rc)
            {
                return rc;
            }
        }
    } while (gc_prev_addr != stop_addr);

gc_done:

    /* Make it possible to detect that gc has finished by writing a
     * gc done ate to the sector. In the field we might have nvs systems
     * that do not have sufficient space to add this ate, so for these
     * situations avoid adding the gc done ate.
     */

    if (fs->ate_wra >= (fs->data_wra + ate_size))
    {
        rc = nvs_add_gc_done_ate(fs);
        if (rc)
        {
            return rc;
        }
    }

    /* Erase the gc'ed sector */
    rc = nvs_flash_erase_sector(fs, sec_addr);
    if (rc)
    {
        return rc;
    }
    return 0;
}

static int nvs_startup(struct nvs_fs *fs)
{
    int rc;
    struct nvs_ate last_ate;
    size_t ate_size, empty_len;
    /* Initialize addr to 0 for the case fs->sector_count == 0. This
     * should never happen as this is verified in nvs_mount() but both
     * Coverity and GCC believe the contrary.
     */
    uint32_t addr = 0U;
    uint16_t i, closed_sectors = 0;
    uint8_t erase_value = fs->flash_params->erase_value;

    NVS_MUTEX_LOCK(fs);

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));
    /* step through the sectors to find a open sector following
     * a closed sector, this is where NVS can write.
     */
    for (i = 0; i < fs->sector_count; i++)
    {
        addr = (i << ADDR_SECT_SHIFT) + (uint16_t)(fs->sector_size - ate_size);
        rc = nvs_flash_cmp_const(fs, addr, erase_value, sizeof(struct nvs_ate));
        if (rc)
        {
            /* closed sector */
            closed_sectors++;
            nvs_sector_advance(fs, &addr);
            rc = nvs_flash_cmp_const(fs, addr, erase_value, sizeof(struct nvs_ate));
            if (!rc)
            {
                /* open sector */
                break;
            }
        }
    }
    /* all sectors are closed, this is not a nvs fs */
    if (closed_sectors == fs->sector_count)
    {
        rc = NVS_ERR_INVALID_DATA;
        goto end;
    }

    if (i == fs->sector_count)
    {
        /* none of the sectors where closed, in most cases we can set
         * the address to the first sector, except when there are only
         * two sectors. Then we can only set it to the first sector if
         * the last sector contains no ate's. So we check this first
         */
        rc = nvs_flash_cmp_const(fs, addr - ate_size, erase_value,
                                sizeof(struct nvs_ate));
        if (!rc)
        {
            /* empty ate */
            nvs_sector_advance(fs, &addr);
        }
    }
    
    /* addr contains address of closing ate in the most recent sector,
     * search for the last valid ate using the recover_last_ate routine
     */
    rc = nvs_recover_last_ate(fs, &addr);
    if (rc)
    {
        goto end;
    }

    /* addr contains address of the last valid ate in the most recent sector
     * search for the first ate containing all cells erased, in the process
     * also update fs->data_wra.
     */
    fs->ate_wra = addr;
    fs->data_wra = addr & ADDR_SECT_MASK;

    while (fs->ate_wra >= fs->data_wra)
    {
        rc = nvs_flash_ate_rd(fs, fs->ate_wra, &last_ate);
        if (rc)
        {
            goto end;
        }

        rc = nvs_ate_cmp_const(&last_ate, erase_value);
        if (!rc)
        {
            /* found ff empty location */
            break;
        }

        if (nvs_ate_valid(fs, &last_ate))
        {
            /* complete write of ate was performed */
            fs->data_wra = addr & ADDR_SECT_MASK;
            /* Align the data write address to the current
             * write block size so that it is possible to write to
             * the sector even if the block size has changed after
             * a software upgrade (unless the physical ATE size
             * will change)."
             */
            fs->data_wra += nvs_al_size(fs, last_ate.offset + last_ate.len);

            /* ate on the last position within the sector is
             * reserved for deletion an entry
             */
            if (fs->ate_wra == fs->data_wra && last_ate.len)
            {
                /* not a delete ate */
                rc = NVS_ERR_INVALID_DATA;
                goto end;
            }
        }

        fs->ate_wra -= ate_size;
    }

    /* if the sector after the write sector is not empty gc was interrupted
     * we might need to restart gc if it has not yet finished. Otherwise
     * just erase the sector.
     * When gc needs to be restarted, first erase the sector otherwise the
     * data might not fit into the sector.
     */
    addr = fs->ate_wra & ADDR_SECT_MASK;
    nvs_sector_advance(fs, &addr);
    rc = nvs_flash_cmp_const(fs, addr, erase_value, fs->sector_size);
    if (rc < 0)
    {
        goto end;
    }
    if (rc)
    {
        /* the sector after fs->ate_wrt is not empty, look for a marker
         * (gc_done_ate) that indicates that gc was finished.
         */
        bool gc_done_marker = false;
        struct nvs_ate gc_done_ate;

        addr = fs->ate_wra + ate_size;
        while ((addr & ADDR_OFFS_MASK) < (fs->sector_size - ate_size))
        {
            rc = nvs_flash_ate_rd(fs, addr, &gc_done_ate);
            if (rc)
            {
                goto end;
            }
            if (nvs_ate_valid(fs, &gc_done_ate) &&
                (gc_done_ate.id == 0xffff) &&
                (gc_done_ate.len == 0U))
            {
                gc_done_marker = true;
                break;
            }
            addr += ate_size;
        }

        if (gc_done_marker)
        {
            /* erase the next sector */
            NVS_LOG_INF("GC Done marker found.\n");
            addr = fs->ate_wra & ADDR_SECT_MASK;
            nvs_sector_advance(fs, &addr);
            rc = nvs_flash_erase_sector(fs, addr);
            goto end;
        }
        NVS_LOG_INF("No GC Done marker found: restarting gc.\n");
        rc = nvs_flash_erase_sector(fs, fs->ate_wra);
        if (rc)
        {
            goto end;
        }
        fs->ate_wra &= ADDR_SECT_MASK;
        fs->ate_wra += (fs->sector_size - 2 * ate_size);
        fs->data_wra = (fs->ate_wra & ADDR_SECT_MASK);

#if CONFIG_NVS_LOOKUP_CACHE
        /**
         * At this point, the lookup cache wasn't built but the gc function need to use it.
         * So, temporarily, we set the lookup cache to the end of the fs.
         * The cache will be rebuilt afterwards
         **/
        for (int i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
        {
            fs->lookup_cache[i].id = NVS_ID_UNUSED;
            fs->lookup_cache[i].addr = NVS_LOOKUP_CACHE_NO_ADDR;
        }
#endif
        rc = nvs_gc(fs);
        goto end;
    }

    /* possible data write after last ate write, update data_wra */
    while (fs->ate_wra > fs->data_wra)
    {
        empty_len = fs->ate_wra - fs->data_wra;

        rc = nvs_flash_cmp_const(fs, fs->data_wra, erase_value, empty_len);
        if (rc < 0)
        {
            goto end;
        }
        if (!rc)
        {
            break;
        }

        fs->data_wra += fs->flash_params->write_unit;
    }

    /* If the ate_wra is pointing to the first ate write location in a
     * sector and data_wra is not 0, erase the sector as it contains no
     * valid data (this also avoids closing a sector without any data).
     */
    if (((fs->ate_wra + 2 * ate_size) == fs->sector_size) &&
        (fs->data_wra != (fs->ate_wra & ADDR_SECT_MASK)))
    {
        rc = nvs_flash_erase_sector(fs, fs->ate_wra);
        if (rc)
        {
            goto end;
        }
        fs->data_wra = fs->ate_wra & ADDR_SECT_MASK;
    }

end:

#if CONFIG_NVS_LOOKUP_CACHE
    if (!rc)
    {
        rc = nvs_lookup_cache_rebuild(fs);
    }
#endif
    /* If the sector is empty add a gc done ate to avoid having insufficient
     * space when doing gc.
     */
    if ((!rc) && ((fs->ate_wra & ADDR_OFFS_MASK) ==
                  (fs->sector_size - 2 * ate_size)))
    {

        rc = nvs_add_gc_done_ate(fs);
    }

    NVS_MUTEX_UNLOCK(fs);
    return rc;
}

int nvs_clear(struct nvs_fs *fs)
{
    int rc;
    uint32_t addr;

    if (!fs->ready)
    {
        NVS_LOG_ERR("NVS not initialized.\n");
        return NVS_ERR_NOT_INIT;
    }

    for (uint16_t i = 0; i < fs->sector_count; i++)
    {
        addr = i << ADDR_SECT_SHIFT;
        rc = nvs_flash_erase_sector(fs, addr);
        if (rc)
        {
            return rc;
        }
    }

    /* nvs needs to be reinitialized after clearing */
    fs->ready = false;

    return 0;
}

int nvs_mount(struct nvs_fs *fs)
{
    int rc;
    size_t write_unit;

    NVS_MUTEX_INIT(fs);

    if (fs->flash_params == NULL)
    {
        NVS_LOG_ERR("Could not obtain flash parameters.\n");
        return NVS_ERR_INVALID_PARAM;
    }
    
    if (!fs->flash_params->read || !fs->flash_params->write || !fs->flash_params->erase)
    {
        NVS_LOG_ERR("Flash parameters error.\n");
        return NVS_ERR_INVALID_PARAM;
    }

    write_unit = fs->flash_params->write_unit;
    /* check that the write block size is supported */
    if (write_unit > NVS_BUFFER_SIZE || write_unit == 0)
    {
        NVS_LOG_ERR("Unsupported write block size.\n");
        return NVS_ERR_INVALID_PARAM;
    }

    /* check that sector size is a multiple of pagesize */
    if (!fs->sector_size || fs->sector_size % fs->flash_params->page_size)
    {
        NVS_LOG_ERR("Invalid sector size.\n");
        return NVS_ERR_INVALID_PARAM;
    }

    /* check the number of sectors, it should be at least 2 */
    if (fs->sector_count < 2)
    {
        NVS_LOG_ERR("Configuration error - sector count.\n");
        return NVS_ERR_INVALID_PARAM;
    }

    NVS_LOG_INF("Starting file system ...\n");
    rc = nvs_startup(fs);
    if (rc)
    {
        return rc;
    }
    NVS_LOG_INF("File system Started up.\n");
    
    /* nvs is ready for use */
    fs->ready = true;

    NVS_LOG_INF("%d Sectors of %d bytes\n", fs->sector_count, fs->sector_size);
    NVS_LOG_INF("ate wra: %d, 0x%04x\n",
            (fs->ate_wra >> ADDR_SECT_SHIFT),
            (fs->ate_wra & ADDR_OFFS_MASK));
    NVS_LOG_INF("data wra: %d, 0x%04x\n",
            (fs->data_wra >> ADDR_SECT_SHIFT),
            (fs->data_wra & ADDR_OFFS_MASK));

    return 0;
}

int nvs_write(struct nvs_fs *fs, uint16_t id, const void *data, size_t len)
{
    int rc, gc_count;
    size_t ate_size, data_size;
    struct nvs_ate wlk_ate;
    uint32_t wlk_addr, rd_addr;
    uint16_t required_space = 0U; /* no space, appropriate for delete ate */
    bool prev_found = false;

    if (!fs->ready)
    {
        NVS_LOG_ERR("NVS not initialized.\n");
        return NVS_ERR_NOT_INIT;
    }

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));
    data_size = nvs_al_size(fs, len);

    /* The maximum data size is sector size - 4 ate
     * where: 1 ate for data, 1 ate for sector close, 1 ate for gc done,
     * and 1 ate to always allow a delete.
     */
    if ((len > (fs->sector_size - 4 * ate_size)) ||
        ((len > 0) && (data == NULL)))
    {
        return NVS_ERR_INVALID_PARAM;
    }

    /* find latest entry with same id */
#if CONFIG_NVS_LOOKUP_CACHE
    if (!nvs_lookup_cache_find(fs, id, &wlk_addr))
    {
        goto no_cached_entry;
    }
#else
    wlk_addr = fs->ate_wra;
#endif
    rd_addr = wlk_addr;

    while (1)
    {
        rd_addr = wlk_addr;
        rc = nvs_prev_ate(fs, &wlk_addr, &wlk_ate);
        if (rc)
        {
            return rc;
        }
        if ((wlk_ate.id == id) && (nvs_ate_valid(fs, &wlk_ate)))
        {
            prev_found = true;
            break;
        }
        if (wlk_addr == fs->ate_wra)
        {
            break;
        }
    }

#if CONFIG_NVS_LOOKUP_CACHE
no_cached_entry:
#endif
    
    if (prev_found)
    {
        /* previous entry found */
        rd_addr &= ADDR_SECT_MASK;
        rd_addr += wlk_ate.offset;

        if (len == 0)
        {
            /* do not try to compare with empty data */
            if (wlk_ate.len == 0U)
            {
                /* skip delete entry as it is already the
                 * last one
                 */
                return 0;
            }
        }
        else if (len == wlk_ate.len)
        {
            /* do not try to compare if lengths are not equal */
            /* compare the data and if equal return 0 */
            rc = nvs_flash_block_cmp(fs, rd_addr, data, len);
            if (rc <= 0)
            {
                return rc;
            }
        }
    }
    else
    {
        /* skip delete entry for non-existing entry */
        if (len == 0)
        {
            return 0;
        }
    }

#if CONFIG_NVS_LOOKUP_CACHE
    if (!prev_found && nvs_lookup_cache_is_full(fs))
    {
        NVS_LOG_DBG("lookup cache full!\n");
        return NVS_ERR_CACHE_FULL;
    }
#endif
    
    
    NVS_LOG_DBG("writing data. id=0x%04x, len=%d.\n", id, len);
    
    /* calculate required space if the entry contains data */
    if (data_size)
    {
        /* Leave space for delete ate */
        required_space = data_size + ate_size;
    }

    NVS_MUTEX_LOCK(fs);

    gc_count = 0;
    while (1)
    {
        if (gc_count == fs->sector_count)
        {
            /* gc'ed all sectors, no extra space will be created
             * by extra gc.
             */
            rc = NVS_ERR_NO_SPACE;
            goto end;
        }

        if (fs->ate_wra >= (fs->data_wra + required_space))
        {
            rc = nvs_flash_wrt_entry(fs, id, data, len);
            if (rc)
            {
                goto end;
            }
            break;
        }

        rc = nvs_sector_close(fs);
        if (rc)
        {
            goto end;
        }

        rc = nvs_gc(fs);
        if (rc)
        {
            goto end;
        }
        gc_count++;
    }
    rc = len;

end:
    NVS_MUTEX_UNLOCK(fs);
    return rc;
}

int nvs_delete(struct nvs_fs *fs, uint16_t id)
{
    return nvs_write(fs, id, NULL, 0);
}

int nvs_read_hist(struct nvs_fs *fs, uint16_t id, void *data, size_t len,
                        uint16_t cnt)
{
    int rc;
    uint32_t wlk_addr, rd_addr;
    uint16_t cnt_his;
    struct nvs_ate wlk_ate;
    size_t ate_size;

    if (!fs->ready)
    {
        NVS_LOG_ERR("NVS not initialized.\n");
        return NVS_ERR_NOT_INIT;
    }

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    if (len > (fs->sector_size - 2 * ate_size))
    {
        return NVS_ERR_INVALID_PARAM;
    }

    cnt_his = 0U;

#if CONFIG_NVS_LOOKUP_CACHE
    if (!nvs_lookup_cache_find(fs, id, &wlk_addr))
    {
        rc = NVS_ERR_NOT_FOUND;
        goto err;
    }
#else
    wlk_addr = fs->ate_wra;
#endif
    rd_addr = wlk_addr;

    while (cnt_his <= cnt)
    {
        rd_addr = wlk_addr;
        rc = nvs_prev_ate(fs, &wlk_addr, &wlk_ate);
        if (rc)
        {
            goto err;
        }
        if ((wlk_ate.id == id) && (nvs_ate_valid(fs, &wlk_ate)))
        {
            cnt_his++;
        }
        if (wlk_addr == fs->ate_wra)
        {
            break;
        }
    }

    if (((wlk_addr == fs->ate_wra) && (wlk_ate.id != id)) ||
        (wlk_ate.len == 0U) || (cnt_his < cnt))
    {
        return NVS_ERR_NOT_FOUND;
    }

    rd_addr &= ADDR_SECT_MASK;
    rd_addr += wlk_ate.offset;
    rc = nvs_flash_rd(fs, rd_addr, data, MIN(len, wlk_ate.len));
    if (rc)
    {
        goto err;
    }

    return wlk_ate.len;

err:
    return rc;
}

int nvs_read(struct nvs_fs *fs, uint16_t id, void *data, size_t len)
{
    int rc;

    rc = nvs_read_hist(fs, id, data, len, 0);
    return rc;
}

int nvs_calc_free_space(struct nvs_fs *fs)
{
    int rc;
    struct nvs_ate step_ate, wlk_ate;
    uint32_t step_addr, wlk_addr;
    size_t ate_size, free_space;
    uint32_t i;

    if (!fs->ready)
    {
        NVS_LOG_ERR("NVS not initialized.\n");
        return NVS_ERR_NOT_INIT;
    }

    ate_size = nvs_al_size(fs, sizeof(struct nvs_ate));

    free_space = 0;
    for (i = 1; i < fs->sector_count; i++)
    {
        free_space += (fs->sector_size - ate_size);
    }

    #if CONFIG_NVS_LOOKUP_CACHE

    struct lookup_cache *cache_entry = fs->lookup_cache;
    for (i = 0; i < CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        if (cache_entry->id == NVS_ID_UNUSED)
        {
            return free_space;
        }
        if ((cache_entry->id != NVS_ID_DELETED))
        {
            rc = nvs_flash_ate_rd(fs, cache_entry->addr, &step_ate);
            if (rc < 0)
            {
                return rc;
            }

            free_space -= nvs_al_size(fs, step_ate.len);
            free_space -= ate_size;
        }
        
        cache_entry++;
    }

    #else

    step_addr = fs->ate_wra;

    while (1)
    {
        rc = nvs_prev_ate(fs, &step_addr, &step_ate);
        if (rc)
        {
            return rc;
        }

        wlk_addr = fs->ate_wra;

        while (1)
        {
            rc = nvs_prev_ate(fs, &wlk_addr, &wlk_ate);
            if (rc)
            {
                return rc;
            }
            if ((wlk_ate.id == step_ate.id) ||
                (wlk_addr == fs->ate_wra))
            {
                break;
            }
        }

        if ((wlk_addr == step_addr) && step_ate.len &&
            (nvs_ate_valid(fs, &step_ate)))
        {
            /* count needed */
            free_space -= nvs_al_size(fs, step_ate.len);
            free_space -= ate_size;
        }

        if (step_addr == fs->ate_wra)
        {
            break;
        }
    }
#endif

    return free_space;   
}

///////////////////////////////////////////////////////////////////////////////
#define NVS_PAGE_SIZE   512
uint8_t flash[NVS_PAGE_SIZE * 3]=
{
0x55, 0x55, 0x55, 0x55, 0x55, 0xff, 0xff, 0xff, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0xff, 0xff,
0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0xff, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88,
0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0xff, 0xff, 0xff, 0xaa, 0xaa, 0xaa, 0xaa,
0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xff, 0xff, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb,
0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xcc, 0xcc, 0xcc, 0xcc,
0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc,
0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xcc, 0xff, 0xff, 0xff,
0xa5, 0xa5, 0xa5, 0xa5, 0xa5, 0xff, 0xff, 0xff, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xff, 0xff,
0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xff, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8,
0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xff, 0xff, 0xff, 0x55, 0x55, 0x55, 0x55,
0x55, 0xff, 0xff, 0xff, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0xff, 0xff, 0x77, 0x77, 0x77, 0x77,
0x77, 0x77, 0x77, 0xff, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x99, 0x99, 0x99, 0x99,
0x99, 0x99, 0x99, 0x99, 0x99, 0xff, 0xff, 0xff, 0xa5, 0xa5, 0xa5, 0xa5, 0xa5, 0xff, 0xff, 0xff,
0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xff, 0xff, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xff,
0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9,
0xa9, 0xff, 0xff, 0xff, 0x55, 0x55, 0x55, 0x55, 0x55, 0xff, 0xff, 0xff, 0x66, 0x66, 0x66, 0x66,
0x66, 0x66, 0xff, 0xff, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0x07, 0x07, 0x04, 0x01, 0x07, 0x00, 0xff, 0xf4, 0x06, 0x06, 0xfc, 0x00, 0x06, 0x00, 0xff, 0x6f,
0x05, 0x05, 0xf4, 0x00, 0x05, 0x00, 0xff, 0xd6, 0x09, 0x09, 0xe8, 0x00, 0x09, 0x00, 0xff, 0xfc,
0x08, 0x08, 0xe0, 0x00, 0x08, 0x00, 0xff, 0x78, 0x07, 0x07, 0xd8, 0x00, 0x07, 0x00, 0xff, 0x19,
0x06, 0x06, 0xd0, 0x00, 0x06, 0x00, 0xff, 0x9d, 0x05, 0x05, 0xc8, 0x00, 0x05, 0x00, 0xff, 0x16,
0x09, 0x09, 0xbc, 0x00, 0x09, 0x00, 0xff, 0x89, 0x08, 0x08, 0xb4, 0x00, 0x08, 0x00, 0xff, 0x0d,
0x07, 0x07, 0xac, 0x00, 0x07, 0x00, 0xff, 0x08, 0x06, 0x06, 0xa4, 0x00, 0x06, 0x00, 0xff, 0x8c,
0x05, 0x05, 0x9c, 0x00, 0x05, 0x00, 0xff, 0x63, 0x09, 0x09, 0x90, 0x00, 0x09, 0x00, 0xff, 0x7b,
0x08, 0x08, 0x88, 0x00, 0x08, 0x00, 0xff, 0xcd, 0x07, 0x07, 0x80, 0x00, 0x07, 0x00, 0xff, 0xfa,
0x06, 0x06, 0x78, 0x00, 0x06, 0x00, 0xff, 0x77, 0x05, 0x05, 0x70, 0x00, 0x05, 0x00, 0xff, 0xce,
0x0c, 0x0c, 0x4c, 0x00, 0x21, 0x00, 0xff, 0xb5, 0x0b, 0x0b, 0x38, 0x00, 0x14, 0x00, 0xff, 0x49,
0x0a, 0x0a, 0x2c, 0x00, 0x0a, 0x00, 0xff, 0x8c, 0x09, 0x09, 0x20, 0x00, 0x09, 0x00, 0xff, 0xba,
0x08, 0x08, 0x18, 0x00, 0x08, 0x00, 0xff, 0x68, 0x07, 0x07, 0x10, 0x00, 0x07, 0x00, 0xff, 0x5f,
0x06, 0x06, 0x08, 0x00, 0x06, 0x00, 0xff, 0xe9, 0x05, 0x05, 0x00, 0x00, 0x05, 0x00, 0xff, 0x50,
0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0x5c, 0xff, 0xff, 0x20, 0x01, 0x00, 0x00, 0xff, 0x2e,

0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0xff, 0xff, 0xff, 0xa5, 0xa5, 0xa5, 0xa5,
0xa5, 0xff, 0xff, 0xff, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xff, 0xff, 0xa7, 0xa7, 0xa7, 0xa7,
0xa7, 0xa7, 0xa7, 0xff, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xff, 0xff, 0xff,
0x55, 0x55, 0x55, 0x55, 0x55, 0xff, 0xff, 0xff, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0xff, 0xff,
0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0xff, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88,
0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0xff, 0xff, 0xff, 0xa5, 0xa5, 0xa5, 0xa5,
0xa5, 0xff, 0xff, 0xff, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xff, 0xff, 0xa7, 0xa7, 0xa7, 0xa7,
0xa7, 0xa7, 0xa7, 0xff, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa9, 0xa9, 0xa9, 0xa9,
0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x09, 0x09, 0x7c, 0x00, 0x09, 0x00, 0xff, 0xd6,
0x08, 0x08, 0x74, 0x00, 0x08, 0x00, 0xff, 0x52, 0x07, 0x07, 0x6c, 0x00, 0x07, 0x00, 0xff, 0x57,
0x06, 0x06, 0x64, 0x00, 0x06, 0x00, 0xff, 0xd3, 0x05, 0x05, 0x5c, 0x00, 0x05, 0x00, 0xff, 0x3c,
0x09, 0x09, 0x50, 0x00, 0x09, 0x00, 0xff, 0x24, 0x08, 0x08, 0x48, 0x00, 0x08, 0x00, 0xff, 0x92,
0x07, 0x07, 0x40, 0x00, 0x07, 0x00, 0xff, 0xa5, 0x06, 0x06, 0x38, 0x00, 0x06, 0x00, 0xff, 0xbf,
0x05, 0x05, 0x30, 0x00, 0x05, 0x00, 0xff, 0x06, 0x09, 0x09, 0x24, 0x00, 0x09, 0x00, 0xff, 0x35,
0x07, 0x07, 0x1c, 0x00, 0x07, 0x00, 0xff, 0xc9, 0x06, 0x06, 0x14, 0x00, 0x06, 0x00, 0xff, 0x4d,
0x05, 0x05, 0x0c, 0x00, 0x05, 0x00, 0xff, 0xc6, 0x09, 0x09, 0x00, 0x00, 0x09, 0x00, 0xff, 0xde,
0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0xff, 0x5c, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,

0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
};

int flash_read (uint32_t addr, void * data, size_t len)
{
    memcpy(data, (uint8_t *)(flash + addr), len);

    return 0;
}

int flash_write (uint32_t addr, const void * data, size_t len)
{
    memcpy((uint8_t *)(flash + addr), data, len);

    return 0;
}

int flash_erase (uint32_t addr, size_t len)
{
    memset((uint8_t *)(flash + addr), 0xff, NVS_PAGE_SIZE);
    
    return 0;
}

const static struct flash_parameters flash_param =
{
4, 0xff, 512,
0, 
flash_read, 
flash_write, 
flash_erase
};

/////////////////////////////////////////////////////////////////////////////////


struct nvs_fs fs = 
{
.offset = 0,
.sector_size = 512,
.sector_count = 3,
.flash_params = &flash_param,
};

uint8_t a5[5] = {0x55, 0x55, 0x55, 0x55, 0x55};
uint8_t a6[6] = {0x66, 0x66, 0x66, 0x66, 0x66, 0x66};
uint8_t a7[7] = {0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77};
uint8_t a8[8] = {0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88};
uint8_t a9[9] = {0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99};
uint8_t aa[10];
uint8_t bb[20];
uint8_t cc[33];

uint8_t aa5[5] = {0xa5, 0xa5, 0xa5, 0xa5, 0xa5};
uint8_t aa6[6] = {0xa6, 0xa6, 0xa6, 0xa6, 0xa6, 0xa6};
uint8_t aa7[7] = {0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7, 0xa7};
uint8_t aa8[8] = {0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8, 0xa8};
uint8_t aa9[9] = {0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9, 0xa9};

#define KEY_5   0x0505
#define KEY_6   0x0606
#define KEY_7   0x0707
#define KEY_8   0x0808
#define KEY_9   0x0909  
#define KEY_A   0x0a0a
#define KEY_B   0x0b0b
#define KEY_C   0x0c0c

static void print_data(void)
{
    for(uint32_t i=0; i<sizeof(flash); i++)
    {
        printf("%02x ", flash[i]);
        if ((i & 0x0f) == 0x0f)
        {
            printf("\r\n");
        }
        if ((i & (NVS_PAGE_SIZE-1)) == NVS_PAGE_SIZE-1)
        {
            printf("\r\n");
        }
    }
    printf("\r\n");
}

static void print_cache(struct nvs_fs *fs)
{
    for(uint32_t i=0; i<CONFIG_NVS_LOOKUP_CACHE_SIZE; i++)
    {
        printf("%d: id=%04x, addr=%08x\n", i, fs->lookup_cache[i].id, fs->lookup_cache[i].addr);
    }
    printf("\r\n");
}

void print_buf(uint8_t *buf)
{
    while(*buf != 0)
    {
        printf("%02x ", *buf);
        buf++;
    }
    printf("\r\n");
}

int main(int argc, char **argv) 
{
    uint32_t i, size;
    char str[20];
    uint32_t cnt=0;
    uint8_t buf[40];
    
    //memset(flash, 0xff, sizeof(flash));

    memset(cc, 0xcc, sizeof(cc));

    // memset(&flash[4], 0x33, 12);
    
    if (nvs_mount(&fs))
    {
        printf("init error!\n");
        return 1;
    }
    
    // print_data();
    // print_cache(&fs);
   
    for (i=0; i<2; i++)
    {
        memset(aa, 0xaa, sizeof(aa));
        memset(bb, 0xbb, sizeof(bb));
        memset(cc, 0xcc, sizeof(cc));
        nvs_write(&fs, KEY_5, a5, sizeof(a5));
        nvs_write(&fs, KEY_6, a6, sizeof(a6));
        nvs_write(&fs, KEY_7, a7, sizeof(a7));
        nvs_write(&fs, KEY_8, a8, sizeof(a8));
        nvs_write(&fs, KEY_9, a9, sizeof(a9));
        nvs_write(&fs, KEY_A, aa, sizeof(aa));
        nvs_write(&fs, KEY_B, bb, sizeof(bb));
        nvs_write(&fs, KEY_C, cc, sizeof(cc));
        
        nvs_write(&fs, KEY_5, aa5, sizeof(aa5));
        nvs_write(&fs, KEY_6, aa6, sizeof(aa6));
        nvs_write(&fs, KEY_7, aa7, sizeof(aa7));
        nvs_write(&fs, KEY_8, aa8, sizeof(aa8));
        nvs_write(&fs, KEY_9, aa9, sizeof(aa9));
        memset(aa, 0xda, sizeof(aa));
        memset(bb, 0xdb, sizeof(bb));
        memset(cc, 0xdc, sizeof(cc));
        nvs_write(&fs, KEY_A, aa, sizeof(aa));
        nvs_write(&fs, KEY_B, bb, sizeof(bb));
        nvs_write(&fs, KEY_C, cc, sizeof(cc));
        
        // print_data();
        // print_cache(&fs);
    }
    
    while(1)
    {
        scanf("%s", str);
        cnt++;
        
        if (strlen(str) != 2)
        {
            printf("input error!");
            continue;
        }
        
        if (str[0] == 'w')  // write
        {
            if (cnt & 1)
            {
                switch(str[1])
                {
                    case '5':
                        nvs_write(&fs, KEY_5, a5, sizeof(a5));
                        break;
                    case '6':
                        nvs_write(&fs, KEY_6, a6, sizeof(a6));
                        break;
                    case '7':
                        nvs_write(&fs, KEY_7, a7, sizeof(a7));
                        break;
                    case '8':
                        nvs_write(&fs, KEY_8, a8, sizeof(a8));
                        break;    
                    case '9':
                        nvs_write(&fs, KEY_9, a9, sizeof(a9));
                        break;    
                    case 'a':
                        memset(aa, 0xaa, sizeof(aa));        
                        nvs_write(&fs, KEY_A, aa, sizeof(aa));
                        break; 
                    case 'b':
                        memset(bb, 0xbb, sizeof(bb));        
                        nvs_write(&fs, KEY_B, bb, sizeof(bb));
                        break;                        
                    case 'c':
                        memset(cc, 0xcc, sizeof(cc));
                        nvs_write(&fs, KEY_C, cc, sizeof(cc));
                        break;                       
                }
            }
            else
            {
                switch(str[1])
                {
                    case '5':
                        nvs_write(&fs, KEY_5, aa5, sizeof(aa5));
                        break;
                    case '6':
                        nvs_write(&fs, KEY_6, aa6, sizeof(aa6));
                        break;
                    case '7':
                        nvs_write(&fs, KEY_7, aa7, sizeof(aa7));
                        break;
                    case '8':
                        nvs_write(&fs, KEY_8, aa8, sizeof(aa8));
                        break;    
                    case '9':
                        nvs_write(&fs, KEY_9, aa9, sizeof(aa9));
                        break;    
                    case 'a':
                        memset(aa, 0xda, sizeof(aa));        
                        nvs_write(&fs, KEY_A, aa, sizeof(aa));
                        break; 
                    case 'b':
                        memset(bb, 0xdb, sizeof(bb));        
                        nvs_write(&fs, KEY_B, bb, sizeof(bb));
                        break;                        
                    case 'c':
                        memset(cc, 0xdc, sizeof(cc));
                        nvs_write(&fs, KEY_C, cc, sizeof(cc));
                        break;                       
                }
            }
            
            // print_data();
            // print_cache(&fs);
        }
        else if (str[0] == 'd')     // delete
        {
            switch(str[1])
            {
                case '5':
                    nvs_delete(&fs, KEY_5);
                    break;
                case '6':
                    nvs_delete(&fs, KEY_6);
                    break;
                case '7':
                    nvs_delete(&fs, KEY_7);
                    break;
                case '8':
                    nvs_delete(&fs, KEY_8);
                    break;    
                case '9':
                    nvs_delete(&fs, KEY_9);
                    break;    
                case 'a':
                    nvs_delete(&fs, KEY_A);
                    break; 
                case 'b':
                    nvs_delete(&fs, KEY_B);
                    break;                        
                case 'c':
                    nvs_delete(&fs, KEY_C);
                    break;                       
            }
            
            // print_data();
            // print_cache(&fs);
        }
        else if (str[0] == 'r')     // read
        {
            int len;
            memset(buf, 0, sizeof(buf));
            switch(str[1])
            {
                case '5':
                    len = nvs_read(&fs, KEY_5, buf, sizeof(a5));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;
                case '6':
                    len = nvs_read(&fs, KEY_6, buf, sizeof(a6));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;
                case '7':
                    len = nvs_read(&fs, KEY_7, buf, sizeof(a7));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;
                case '8':
                    len = nvs_read(&fs, KEY_8, buf, sizeof(a8));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;    
                case '9':
                    len = nvs_read(&fs, KEY_9, buf, sizeof(a9));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;    
                case 'a':     
                    len = nvs_read(&fs, KEY_A, buf, sizeof(aa));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break; 
                case 'b':     
                    len = nvs_read(&fs, KEY_B, buf, sizeof(bb));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;                        
                case 'c':
                    len = nvs_read(&fs, KEY_C, buf, sizeof(cc));
                    if (len <= 0)
                    {
                        printf("not found!");
                    }
                    else
                    {
                        print_buf(buf);
                    }
                    break;                     
            }
        }
        else if (str[0] == 'p')     // print
        {
            switch(str[1])
            {
                case 'd':
                    print_data();
                    break;
                case 'c':
                    print_cache(&fs);
                    break;
                case 'f':
                    size = nvs_calc_free_space(&fs);
                    printf("nvs free size: %d bytes\n", size);
                    break; 
            }
        }       
    }
    
    return 0;
}
